#include "VisualSensor.h"
#include "ocam_functions.h" // by Scaramuzza

using namespace std;
using namespace cv;

Mat captured;
VideoCapture capture;
struct ocam_model my_ocam;

// FOR DEBUGGING OUTPUT
Mat cdst;

int cam_index = 1;

void undistorted_perspective(Mat & frame, IplImage * dst_persp, struct ocam_model & my_ocam)
{
  CvMat* mapx_persp = cvCreateMat(dst_persp->height, dst_persp->width, CV_32FC1);
  CvMat* mapy_persp = cvCreateMat(dst_persp->height, dst_persp->width, CV_32FC1);
  float sf = 10;
  create_perspecive_undistortion_LUT( mapx_persp, mapy_persp, & my_ocam, sf );

  IplImage* tmp = new IplImage(frame);
  cvRemap( tmp, dst_persp, mapx_persp, mapy_persp, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0) );

  //cvNamedWindow( "Undistorted Perspective Image", 1 );
  //cvShowImage( "Undistorted Perspective Image", dst_persp );

  delete tmp;
  cvReleaseMat(&mapx_persp);
  cvReleaseMat(&mapy_persp);
}

void undistorted_panorama(Mat & frame, int xc, int yc)
{
  CvSize size_pan_image = cvSize(1200,400);        // size of the undistorted panoramic image
  IplImage *dst_pan     = cvCreateImage( size_pan_image, 8, 3 );    // undistorted panoramic image

  CvMat* mapx_pan   = cvCreateMat(dst_pan->height, dst_pan->width, CV_32FC1);
  CvMat* mapy_pan   = cvCreateMat(dst_pan->height, dst_pan->width, CV_32FC1);

  float Rmax = 470;  // the maximum radius of the region you would like to undistort into a panorama
  float Rmin = 20;   // the minimum radius of the region you would like to undistort into a panorama  
  create_panoramic_undistortion_LUT ( mapx_pan, mapy_pan, Rmin, Rmax, xc, yc);

  IplImage* tmp = new IplImage(frame);
  cvRemap( tmp, dst_pan  , mapx_pan, mapy_pan, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0) );

  //cvNamedWindow( "Undistorted Panoramic Image", 1 );
  //cvShowImage( "Undistorted Panoramic Image", dst_pan );

  delete tmp;
  cvReleaseImage(&dst_pan);
  cvReleaseMat(&mapx_pan);
  cvReleaseMat(&mapy_pan);
}

/*
	Metodo per acquisire un frame
*/
void acquire_img(VideoCapture & capture, Mat & frame)
{
  capture >> frame;
  //imshow("Webcam", frame);
  //waitKey();
}

/*
	Metodo per il flip sinistra - destra
*/
void flip_img(Mat & frame)
{
  flip(frame, frame, 1);
}

#if 1
void updateCapture(){
	
	Mat frame;
	for (int i = 0; i < 15; i++) // il buffer è lungo 6
	{
	  capture.grab();
	}
	
	//capture.release();
	//waitKey(1000);
	//capture = VideoCapture(cam_index);

	capture >> frame;
	
	Mat undist_frame;
	Mat gray_undist_frame;
	Mat bw_undist_frame;
	flip_img(frame);
	undistorted_panorama(frame, my_ocam.xc, my_ocam.yc);
	IplImage * dst_persp = cvCreateImage(frame.size(), 8, 3 ); // undistorted frame
	undistorted_perspective(frame, dst_persp, my_ocam);
	undist_frame = cvarrToMat(dst_persp);
	cvtColor(undist_frame, gray_undist_frame, CV_RGB2GRAY);
	threshold(gray_undist_frame, bw_undist_frame, 215, 255, 0); // alzata soglia per ostacoli (era 210)
	cvReleaseImage(&dst_persp);
	
	bw_undist_frame.copyTo(captured);
	cvtColor(captured, cdst, CV_GRAY2BGR);

}
#else
void updateCapture(){}
#endif


/*
	Ritorna le linee trovate con Hough che rispettano i parametri:
	rho, teta, density, srn, stn, m_margin, q_margin

	Dove:
	
	- rho, teta, density, srn ed stn sono parametri standard della
	funzione HoughLines di OpenCV;
	- m_margin è la soglia per cui due linee che hanno coefficienti angolari
	che differiscono meno di m_margin vengono considerate una sola linea;
	
	- q_margin è la soglia per cui due linee che hanno termine noto che
	deifferisce meno di q_margin vengono considerate una sola linea.

*/
vector<Vec2f> getLines(Mat dst)
{

	int rho = 1,
	teta = 2,
	density = 290,
	srn = 0,
	stn = 0,
	m_margin = 38,
	q_margin = 255;

	vector<Vec2f> lines;
	double max = 0;

	// individuo le linee rette con la trasformata di Hough
	HoughLines(dst, lines, rho, teta*CV_PI/180, density, srn, stn );

	// fondo le linee con q ed m che differiscono di poco (m_margin, q_margin)
	// tenendo la media dei q e degli m
	
	vector<Vec2f> filtered_lines;
	vector<int> filtered_lines_counts; // conta quante rette ho aggiunto ad ogni gruppo

	for( size_t i = 0; i < lines.size(); i++ )
	{
		double rho1 = lines[i][0];
		double theta1 = lines[i][1];
		
		bool added = false;
		
		for( size_t j = 0; j < filtered_lines.size(); j++ )
		{
			double rho2 = filtered_lines[j][0];
			double theta2 = filtered_lines[j][1];

			if(abs(rho1-rho2/filtered_lines_counts[j]) < q_margin and abs(theta1-theta2/filtered_lines_counts[j]) < m_margin*CV_PI/180)
			{
				added = true;
				filtered_lines[j][0] += rho1;
				filtered_lines[j][1] += theta1;
				filtered_lines_counts[j] += 1;
				break;
			}
		}

		// se non l'ho aggiunta ad un gruppo di rette simili allora creo un nuovo gruppo che
		// al momento conterrà soltanto questa retta.
		if (not added)
		{
			filtered_lines.push_back(lines[i]);
			filtered_lines_counts.push_back(1);
		}
	}

	// calcolo la media degli m e dei q di tutti i gruppi
	for( size_t i = 0; i < filtered_lines.size(); i++ )
	{
		filtered_lines[i][0] = filtered_lines[i][0]/filtered_lines_counts[i];
		filtered_lines[i][1] = filtered_lines[i][1]/filtered_lines_counts[i];
	}

	// for DEBUGGING preview
	for( size_t i = 0; i < filtered_lines.size(); i++ )
	{
		float rho = filtered_lines[i][0], theta = filtered_lines[i][1];
		Point pt1, pt2;
		double a = cos(theta), b = sin(theta);
		double x0 = a*rho, y0 = b*rho;
		pt1.x = cvRound(x0 + 1000*(-b));
		pt1.y = cvRound(y0 + 1000*(a));
		pt2.x = cvRound(x0 - 1000*(-b));
		pt2.y = cvRound(y0 - 1000*(a));
		line( cdst, pt1, pt2, Scalar(0,0,255), 1, CV_AA);
	}

	imshow("debug", cdst);

	return filtered_lines;
}


/*
	Ritorna true se tra le rette viste ne è presente almeno una di "quasi" orizzontale.
*/
bool VisualSensor::isAligned()
{
	updateCapture();
	
	double threshold = CV_PI/10; // precisione di allineamento desiderata
	
	vector<Vec2f> lines = getLines(captured);
	
	cout << "Is aligned CALLED!" << endl;

	imshow("debug",cdst);

	// Output di DEBUG
	waitKey(33);
	
	for( size_t i = 0; i < lines.size(); i++ )
		if(abs(lines[i][1] - CV_PI/2) < threshold)
			return true;

	return false;
}

/*
	Date due linee nel formato ritornato da HoughLines
	mette il loro punto di intersezione in Point intersection.
*/
bool getIntersection(Point& intersection, Vec2f l1, Vec2f l2,double tolerance=0.01)
{

	// TODO: REGOLARE TOLERANCE
	double m1,q1,m2,q2;

	// calcolo i coefficienti angolari ed i termini noti
	m1 = -cos(l1[1])/sin(l1[1]);
	m2 = -cos(l2[1])/sin(l2[1]);
	q1 = l1[0]/sin(l1[1]);
	q2 = l2[0]/sin(l2[1]);

	// verifico non siano parallele
	if(abs(m1-m2) < tolerance)
	{
		cout << "Richiesta intersezione di rette quasi parallele (m1,m2):" << m1 << "," << m2 << ")" << endl;
		return false;
	}
	
	// calcolo l'intersezione
	intersection.x = cvRound( (q2-q1) / (m1-m2) );
	intersection.y = cvRound( (m1*q2-m2*q1) / (m1-m2) );

	// DEBUGGING OUTPUT
	circle(cdst, intersection, 3, Scalar(0,0,255), 10, 8, 0);

	imshow("debug", cdst);

	return true;
}

/*
	Calcola il punto medio della retta (considera il segmento creato dalle intersezioni
	dei lati della cella) più orizzontale tra quelle viste,
	che si suppone sia la retta verso cui il robot deve andare se è
	allineato con la prossima cella.
*/
bool getNextLineCenter(Point & center)
{
	// assumo che il robot sia allineato
	// assumo sia visibile sempre e solo la linea frontale, mai quella posteriore

	// trovo le linee
	vector<Vec2f> lines = getLines(captured);
	Vec2f ref_line;
	ref_line[1] = 0; // la setto con valore minimo e cerco la più orizzontale possibile

	// trovo la linea più vicina all'orizzontalità (minima distanza da pi/2)
	for( size_t i = 0; i < lines.size(); i++ )
		if(abs(lines[i][1] - CV_PI/2) < abs(ref_line[1] - CV_PI/2))
			ref_line = lines[i];

	// cerco le intersezioni ESTREME sulla linea orizzontale ref_line
	Point min_x_inters;
	Point max_x_inters;
	min_x_inters.x = captured.cols;
	
	for( size_t i = 0; i < lines.size(); i++ )
	{
		Point intersection;
		if(getIntersection(intersection, lines[i], ref_line))
		{
			if(intersection.x < min_x_inters.x)
				min_x_inters = intersection;
			if(intersection.x > max_x_inters.x)
				max_x_inters = intersection;
		}
	}

	// se le intersezioni estreme non sono abbastanza estreme segnalo un errore
	if(min_x_inters.x > captured.cols/2. or max_x_inters.x < captured.cols/2.)
	{
		cout << "Intersezioni sulla linea di orizzonte non valide!" << "(" << ref_line[0] << "," << ref_line[1] << ") " << endl;
		return false;
	}

	// salvo il punto medio
	center.x = (min_x_inters.x+max_x_inters.x)/2.;
	center.y = (min_x_inters.y+max_x_inters.y)/2.;

	// DEBUG OUTPUT
	circle(cdst, center, 3, Scalar(0,255,0), 15, 8, 0);

	imshow("debug",cdst);
	
	return true;
}

/*
 Ritorna la percentuale di ascisse percorse dal centro della linea frontale..

 Se non lo trova lo assume lontano ma centrato, quindi ritorna 0.5

*/ 
double VisualSensor::getCenterXRatio() {

	updateCapture();
	
	Point center;
	bool result = getNextLineCenter(center);

	// DEBUG PREVIEW
	waitKey(33);

	double centerXRatio = result ? (double)center.x/captured.cols : 0.5;

	cout << "centerXRatio: " << centerXRatio << endl;

	return centerXRatio;
}

/*

	Ritorna la percentuale di ordinate percorse dal centro della linea frontale.
	se non trova il centro lo assume molto lontano (ad y negative) e quindi ritorna 0.

*/
double VisualSensor::getCenterYRatio() {
	updateCapture();
	
	Point center;
	bool result = getNextLineCenter(center);

	// DEBUG PREVIEW
	waitKey(33);

	double centerYRatio = result ? (double)center.y/captured.rows : 0;

	cout << "centerYRatio: " << centerYRatio << endl;

	return centerYRatio;
}

/*
	Distanza in pixel eventualmente da convertire in metri
	in base all'altezza della telecamera h e
	un parametro alfa che dipende dalla forma dello specchio
	e dalla distanza camera-specchio

	alfa dipende anche dalla scala dell'immagine in px, 
	nell'immagine originaria una cella è molto piccola
*/
double cvt_pixel2meters(int px)
{
  double h = 0.07; // distance in meters from ground to mirror sphere center 
  double alfa = 5; //95; // in px
  return h * tan(px / alfa);
}

VisualSensor::VisualSensor()
{}

VisualSensor::VisualSensor(std::vector<char, std::allocator<char> > filename)
{

	capture = VideoCapture(cam_index);

	// Read the parameters of the omnidirectional camera from the TXT file
	filename.push_back('\0');
	get_ocam_model(&my_ocam, &filename[0]); // from std::string to char*

	// Print ocam_model parameters

    printf("pol =\n");    for (int i=0; i<my_ocam.length_pol; i++){    printf("\t%e\n",my_ocam.pol[i]); };    printf("\n");
    printf("invpol =\n"); for (int i=0; i<my_ocam.length_invpol; i++){ printf("\t%e\n",my_ocam.invpol[i]); }; printf("\n");  
    printf("\nxc = %f\nyc = %f\n\nwidth = %d\nheight = %d\n",my_ocam.xc,my_ocam.yc,my_ocam.width,my_ocam.height);

    // Da 3D a 2D, cioè dal vettore della sfera all' immagine (pixel)
    // da 2D a 3D normalizzato

    double point3D[3] = { 100 , 200 , -300 };       // a sample 3D point
  	double point2D[2];                              // the image point in pixel coordinates  
  	world2cam(point2D, point3D, &my_ocam); // The behaviour of this function is the same as in MATLAB
  	std::cout << "point2D " << point2D[0] << " " << point2D[1] << std::endl;
  	cam2world(point3D, point2D, &my_ocam);
  	std::cout << "point3D " << point3D[0] << " " << point3D[1] << " " << point3D[2] << std::endl; // normalizzati
	
	/********** DEBUGGING AREA **********
	// debug acquisition simulation
  	
  	vector<string> img_path;
	img_path.push_back("/home/renzo/Desktop/hugh_grid/1.jpg");
	img_path.push_back("/home/renzo/Desktop/hugh_grid/2.jpg");
	img_path.push_back("/home/renzo/Desktop/hugh_grid/3.jpg");


	int i = 0;

	while(i < 3)
	{

		Mat src = imread(img_path[i], 0);
		imshow("src", src);
		cvtColor(src, cdst, CV_GRAY2BGR);
 		if(src.empty())
 		{
    		cout << "can not open " << img_path[i] << endl;
    	}
    	else
		{
			Point p;
			captured = src;
			cout << isAligned() << endl;
			cout << getNextLineCenter(p) << endl;
			cout << "(" << p.x << "," << p.y << ")" << endl;
		}

		waitKey();

		i++;
	}

	/*************************/
}